#include "BucklingSolver.h"

#include "Utility/Exception/SGException.h"
#include "Utility/Algebra/Config.h"
#include "Utility/Logging/SGLogger.h"
#include "Utility/Algebra/sgArpack.h"

#include "DBManager/IService/Input/Dynamic/IEIGRLService.h"
#include "DBManager/IService/Input/Element/IElementService.h"
#include "DBManager/IService/Result/Nodal/IDISPLACEMENTService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"

#include "SGFem/ElementCalculator/Factory/ElementCalculatorFactory.h"
#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/DataAdapter.h"
#include "SGFem/Task/Kernel/MModel.h"
#include "SGFem/Task/Static/StaticSolver.h"

using namespace SG::Algebra;
using SG::DataStructure::Common::Real;
using SG::DataStructure::Common::Status;
using SG::DataStructure::FEM::SOLType;
using SG::DataStructure::FEM::LoadCombination;
using SG::DataStructure::FEM::Project;
using SG::DataStructure::FEM::Subcase;

using namespace SG::DBManager;

using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::EigenSparse;
using SG::FEMSolver::EigenVector;
using SG::FEMSolver::Element::ElementCalculatorFactory;

void BucklingSolver::Solve (const SG::DBManager::DBServiceFactorySP dbServiceFactory,
                            const SG::DataStructure::FEM::Project&        project,
                            int                                           subcaseId)
{
    // 当前的分析类型
    SG::DataStructure::FEM::SOLType analysisType = SOLType::SOL_SEBUCKL;

    // 查询对应的线性分析工况 `STATSUB`
    auto para = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (subcaseId);
    Subcase subcase(para);
    const auto staticSubId = subcase.GetStaticSubId ();

    // 求解阶数
    int nd = 0;
    // https://nexus.hexagon.com/documentationcenter/zh-CH/bundle/MSC_Nastran_2024.1/page/Nastran_Combined_Book/qrg/casecontrol4a/TOC.METHOD.Case.xhtml
    SG::DataStructure::FEM::EIGRLData eigrlData;
    if (Status::STATUS_Success == dbServiceFactory->get<IEIGRLService> ()->FindBySId (subcase.GetMethodId (), eigrlData))
    {
        nd = eigrlData.m_ND;
    }
    // TODO(wuxingchuang) 后续还需要考虑EIGB
    // SG::DataStructure::FEM::EIGBData eigbData;
    // if (dbServiceFactory->get<EIGBService> ()->FindById (subcase.GetMethodId (), eigrlData))
    // {
    //     nd = eigbData.m_ND;
    // }

    if (nd <= 0)
    {
        throw SG::Utility::ValueError ("求解阶数为0，请检查输入参数。subcaseId：" + std::to_string (subcaseId));
    }

    // 获取静力工况位移
    const auto& dispRepository = dbServiceFactory->get<IDISPLACEMENTService> ();
    std::vector<SG::DataStructure::Post::DISPLACEMENTData> displacementDatas;
    auto hasStaticResult = dispRepository->Find ({SOLType::SOL_SESTATIC, staticSubId}, _OUT displacementDatas);
    
    // 没有staticSubId对应的线性静力分析结果，执行静力计算&获取节点位移向量
    if (Status::STATUS_Failure == hasStaticResult)
    {
        // 执行静力分析
        StaticSolver solver;
        solver.Solve (dbServiceFactory, project, { static_cast<SG::DataStructure::Common::Id> (subcaseId) });

        if (Status::STATUS_Failure == dispRepository->Find ({SOLType::SOL_SESTATIC, subcaseId}, _OUT displacementDatas))
        {
            SGError << "can not find static analysis result in buckling analysis ";
            return;
        }
    }

    std::vector<Real> displacement;
    SG::FEMSolver::DataAdapter::UnpackGridField (displacementDatas, _OUT displacement);


    // 计算应力刚度矩阵
    SGInfo << "compute stress Stiffness \n";
    auto elementTypes = dbServiceFactory->get<IElementService> ()->GetAllElementTypes ();
    for (const auto& type : elementTypes)
    {
        auto eleCalculator = ElementCalculatorFactory::Create (type);
        if (NULL == eleCalculator)
        {
            SGWarn << "can 't find element calc " << std::endl;
        }

        eleCalculator->Initialize (type);
        eleCalculator->ComputeStressStiffness (dbServiceFactory, staticSubId);
    }

    // 此时必须静力分析得到的应力刚度并没有释放
    auto& assembly = AssemblyEigen::GetInstance ();
    auto K = assembly.GetStiffnessMatrix ();
    auto KSigma = assembly.AssembleStressStiffnessMatrix ();

    
    // 屈曲特征值计算
    std::vector<Real_t>          eigenvalues;
    std::vector<Vec_t>           eigenvectors;
    
#if 0
    auto K_norm = K.norm ();
    auto KSigma_norm = KSigma.norm ();
    SG::Algebra::TArpack<Real_t> eigSolver;
    auto                         opt = eigSolver.getDefaultOption ();
    opt.nev                          = nd;                                 // Number of eigenvalues of OP to be computed. 0 < NEV < N.
    opt.mode                         = SG::Algebra::arpack::Mode::MODE_3;  // default
    opt.which                        = "SM";                               // Specify which of the Ritz values of OP to compute.
    opt.sigma                        = -KSigma_norm / K_norm;

    eigSolver.setOption (opt);

    eigSolver.solve (KSigma, K, _OUT eigenvalues, _OUT eigenvectors);
    if (0 != eigSolver.info ())
    {
        SGError << "equation solve failed !!!";
    }

    for (int i = 0; i < eigenvalues.size (); ++i)
    {
        eigenvalues[i] = -1.0 / eigenvalues[i];
    }
#else
    SG::Algebra::TArpack<Real_t> eigSolver;
    auto                         opt = eigSolver.getDefaultOption ();
    opt.nev                          = nd;    // Number of eigenvalues of OP to be computed. 0 < NEV < N.
    opt.mode                         = SG::Algebra::arpack::Mode::MODE_4;
    opt.which                        = "LM";  // Specify which of the Ritz values of OP to compute.
    opt.sigma                        = 0.0;

    eigSolver.setOption (opt);

    eigSolver.solve (KSigma, K, _OUT eigenvalues, _OUT eigenvectors);

    if (0 != eigSolver.info ())
    {
        SGError << "equation solve failed !!!";
    }

    for (int i = 0; i < eigenvalues.size (); ++i)
    {
        eigenvalues[i] = -1.0 / eigenvalues[i];
    }
#endif

    Kernel::MModel model (analysisType, dbServiceFactory);
    
    // save result
    int m = K.rows ();
    SG::Algebra::Mat_t phi (m, nd);  // EigenVector
    SG::Algebra::Vec_t lambda (nd);  // EigenValues
    for (int i = 0; i < nd; ++i)
    {
        // 保存特征值和屈曲模态
        phi.col (i) = eigenvectors[i];
        lambda (i)  = eigenvalues[i];

        Real gMass  = phi.col (i).transpose () * KSigma * phi.col (i);
        Real gStiff = phi.col (i).transpose () * K * phi.col (i);

        model.SetEigenValue (i + 1, lambda[i], gMass, gStiff, 0, 0);
    }

    model.SetEigenVector (phi, lambda, subcaseId);

    SGInfo << "Buckling Solve fini  \n";
}